#include "FindAngels.h"
#include <opencv2\core\core.hpp>

FindAngels::FindAngels(void) : Filter(2, 1)
{
	outputs[0] = cv::Mat(1,2, CV_32FC1);
}

FindAngels::~FindAngels(void)
{
}

void FindAngels::Process()
{
	float Ax, Ay, Az;
	float Bx, By;
	float alpha, beta;

	float *in1 = inputs[0]->ptr<float>(0);
	Ax = in1[0];
	Ay = in1[1];
	Az = in1[2];

	float *in2 = inputs[1]->ptr<float>(0);
	Bx = in2[0];
	By = in2[1];

	alpha = atan((Bx - Ax) / Az);
	beta = atan((By - Ay) / Az);

	float *out = outputs[0].ptr<float>(0);
	out[0] = alpha;
	out[1] = beta;
}
